/**
*  \file cosdoc.c
*
*  \brief CAN Open SDO Client
*
*  \author  Michael J. Hohmann <michael.hohmann@linde-mh.de>
*           Michael Kunkelmann <michael.kunkelmann@linde-mh.de>
*/

/*===============================================[ private includes  ]================================================*/
#include <stddef.h>
#include "cosdoc.h"
#include "../endian/endian.h"
#include <string.h>

/*===============================================[ private defines   ]================================================*/
#define EXPEDITED_MAX_SIZE U8_CONST( 4 )

#define EXTRACT_SCS(arg)   ((arg)>>5)
#define EXTRACT_E(arg)     (!!((arg)&0x02))
#define EXTRACT_S(arg)     ((arg)&0x01)
#define EXTRACT_N(arg)     (4-(((arg)>>2)& 0x03))

/*===============================================[ private datatypes ]================================================*/

/** Client Command Specifier */
enum ccs
{
   download_segment_request   = 0x00,
   initiate_download_request  = 0x01,
   initiate_upload_request    = 0x02,
   upload_segment_request     = 0x03,
   abort_transfer_request     = 0x04
};

/** Server Command Specifier */
enum scs
{
   upload_segment_response    = 0x00,
   download_segment_response  = 0x01,
   initiate_upload_response   = 0x02,
   initiate_download_response = 0x03
};

/*===============================================[ private variables ]================================================*/

/*===============================================[ public variables  ]================================================*/

/*===============================================[ private functions ]================================================*/

static U8 cosdoc_pack_cmd( U8 cmd,U8 n,BO e,U8 s);
STATIC_INLINE U32 cosdoc_get_mux( U8 data[8] );
static void cosdoc_abort_request(struct cosdoc *me, U32 reason );
static void reentrant_get(void* d, void *s, U32 size );
static void cosdoc_start_timer(struct cosdoc *me );
static void cosdoc_stop_timer(struct cosdoc *me );
static void cosdoc_reload_timer(struct cosdoc *me );

static RC cosdoc_state_idle( struct cosdoc *me, U8 data[8]);
static RC cosdoc_state_initiate_sdo_upload( struct cosdoc *me, U8 data[8]);
static RC cosdoc_state_sdo_upload_normal_transfer( struct cosdoc *me, U8 data[8]);
static RC cosdoc_state_initiate_sdo_download( struct cosdoc *me, U8 data[8]);
static RC cosdoc_state_abort_transfer_request(struct cosdoc *me, U8 data[8]);
static RC cosdoc_state_download_sdo_segment_protocol( struct cosdoc *me, U8 data[8]);

static BO process_scs(  struct cosdoc *me, enum scs expected, U8 data[8] );
static BO process_toggle_bit(  struct cosdoc *me, U8 data[8] );
static BO process_mux(  struct cosdoc *me, U8 data[8] );
static RC send_download_segment_request( struct cosdoc *me);
static RC cosdoc_transmit(struct cosdoc *me, U8 data[8] );

/*====================================================================================================================*/

RC cosdoc_initialize
(
   struct cosdoc *me,
   cosdoc_transmit_function_t transmit,
   void* callback_data,
   cosdoc_abort_function_t abort,
   void *abort_data,
   U32 timeout
)
{
	memset( me, 0, sizeof(struct cosdoc) );
	me->transmit = transmit;
	me->transmit_data = callback_data;
	me->abort = abort;
	me->abort_data = abort_data;
	me->state = cosdoc_state_idle;
	me->timeout_max = timeout;
	return RC_SUCCESS;
}

void cosdoc_process( struct cosdoc *me, U8 data[8])
{
   memcpy( &me->incomming_message[0], &data[0], 8 );
   me->message_pending=TRUE;
}

/*
|| 7..5  || 4  ||  3..2 || 1  || 0 ||
|| ccs=1 || X  ||  n    || e  || s ||
*/
RC cosdoc_download( struct cosdoc *me, U32 mux, U32 size, struct fd *fh )
{
	RC rv = RC_SUCCESS;
   U16 index;
   U8 subindex;
   index = (U16)(mux>>8);
   subindex = mux & 0xff;

	if( cosdoc_state_idle == me->state )
	{
		U8 req[8];
      memset(&req[0],0x00,8);

		if(  EXPEDITED_MAX_SIZE >= size )
		{
		   /* Expedited Transfer */
		   req[0] = cosdoc_pack_cmd(initiate_download_request,EXPEDITED_MAX_SIZE-(U8)size,TRUE,TRUE);
		   endian_copy_to_little(&req[1],&index,2);
		   req[3]=subindex;
		   me->size=0;
		   fd_open( fh,"r");
		   fd_read( fh,&req[4],size);
		   fd_close( fh, fd_close_status_success );
		}
		else
		{
		   /* Normal Transfer */
         me->file = fh;
		   fd_open( me->file,"r");
		   req[0] = cosdoc_pack_cmd(initiate_download_request,0,FALSE,TRUE);
         endian_copy_to_little(&req[1],&index,2);
         req[3] = subindex;
         me->size = size;
         me->toggle_bit = FALSE ;
         endian_copy_to_little(&req[4],&size,sizeof(U32));

		}
      me->current_mux = mux;
		me->state = cosdoc_state_initiate_sdo_download;
      me->transmission_status = cosdoc_transmission_status_pending;
      cosdoc_transmit( me, req);
      cosdoc_start_timer( me );
	}
	else
	{
		rv=RC_ERROR_BUSY;
	}
	return rv;
}


/* Request data from server. */
RC cosdoc_upload( struct cosdoc *me, U32 mux, struct fd *fh )
{
   RC rv=RC_SUCCESS;

   if( cosdoc_state_idle == me->state)
   {
      U8 req[8];
      U16 index;
      U8 subindex;
      index = (U16) (mux>>8);
      subindex = mux & 0xff;
      me->current_mux = mux;

      req[0] = cosdoc_pack_cmd( initiate_upload_request, 0, FALSE, FALSE );
      endian_copy_to_little(&req[1],&index,2);
      req[3] = subindex;
      memset(&req[4],0,4);
      me->state = cosdoc_state_initiate_sdo_upload;
      me->transmission_status=cosdoc_transmission_status_pending;
      cosdoc_transmit( me, req );
      me->file = fh;
      cosdoc_start_timer( me );

      rv = fd_open( me->file,"w");
   }
   else
   {
      rv = RC_ERROR_BUSY;
   }
   return rv;
}

enum transmission_status cosdoc_get_status(struct cosdoc *me)
{
   return me->transmission_status;
}

RC cosdoc_abort(struct cosdoc *me, U32 mux, U32 code )
{
   U8 req[8];
   U16 index;
   U8 subindex;
   index = (U16) (mux >> 8);
   subindex = mux & 0xff;

   /* reset communication status */
   me->state=cosdoc_state_idle;
   me->transmission_status = cosdoc_transmission_status_idle;
   me->message_pending = FALSE;
   memset(req,0,sizeof(req));
   req[0] = abort_transfer_request << 5;
   endian_copy_to_little(&req[1],&index,sizeof(index));
   req[3] = subindex;
   endian_copy_to_little(&req[4],&code,sizeof(code));

   return cosdoc_transmit( me, req );
}

void cosdoc_timer_service( struct cosdoc *me, U32 ticks )
{
   if( me->timer_active )
   {
      if( 0 != me->count_down )
      {
         if( me->count_down > ticks)
         {
            me->count_down -= ticks;
         }
         else
         {
            me->communication_timed_out = TRUE;
            me->timer_active=FALSE;
            me->count_down=0;
         }
      }
   }
}

/** idle callback function.
*
*  This function will be called by the idle tasklet.
*
*  \param me Pointer to the calling tasklet.
*/
void cosdoc_idle_service( struct cosdoc *me )
{
   if( me->communication_timed_out )
   {
      me->communication_timed_out = FALSE;
      cosdoc_abort_request( me, COSDO_ABORT_SDO_PROTOCOL_TIMED_OUT );
   }

   if( me->message_pending )
   {
      cosdoc_state_function_t state;
      U8 data[8];

      reentrant_get( &data[0], &me->incomming_message[0], 8 );
      me->message_pending=FALSE;

      if( abort_transfer_request==EXTRACT_SCS( data[0]) )
      {
         me->state=cosdoc_state_abort_transfer_request;
      }

      reentrant_get(&state, &me->state, sizeof(me->state) );

      if( cosdoc_state_idle != me->state )
      {
         cosdoc_reload_timer(me);
      }

      state(me, &data[0]);
   }
}

/*===============================================[ private functions ]================================================*/


/** Idle State Function */
static RC cosdoc_state_idle( struct cosdoc *me, U8 data[8])
{
   UNREFERENCED(me);
   UNREFERENCED(data);
   return RC_SUCCESS;
}

static RC cosdoc_state_initiate_sdo_upload( struct cosdoc *me, U8 data[8])
{
   if( !process_scs( me, initiate_upload_response, data ))
   {
      return RC_ERROR;
   }

   if( !process_mux( me, data ) )
   {
      return RC_ERROR;
   }

   if( EXTRACT_E(data[0]) ) /* Transfer type  */
   {
      me->size = EXTRACT_N(data[0]);
      if(RC_SUCCESS != fd_write( me->file, &data[4], me->size ))
      {
         fd_close( me->file, fd_close_status_abort );
         cosdoc_abort_request( me, COSDO_ABORT_DATA_TYPE_DOES_NOT_MATCH_LENGTH_OF_SERVICE_PARAMETER_TOO_HIGH );
      }
      else
      {
         me->state = cosdoc_state_idle;
         me->transmission_status=cosdoc_transmission_status_completed;
         cosdoc_stop_timer( me );
         fd_close( me->file, fd_close_status_success );
      }
   }
   else
   {
      if( EXTRACT_S(data[0]) ) /* size indicator */
      {
         struct fd_stat stat;

         endian_copy_from_little( &me->size, &data[4], sizeof(U32) );

         fd_stat( me->file, &stat );

         if( me->size < stat.size_min )
         {
            cosdoc_abort_request( me, COSDO_ABORT_DATA_TYPE_DOES_NOT_MATCH_LENGTH_OF_SERVICE_PARAMETER_TOO_LOW );
         }
         else
         {
            if( me->size > stat.size_max )
            {
               cosdoc_abort_request( me, COSDO_ABORT_DATA_TYPE_DOES_NOT_MATCH_LENGTH_OF_SERVICE_PARAMETER_TOO_HIGH );
            }
            else
            {
               U8 res[8];
               memset(res,0,8);
               res[0]= upload_segment_request << 5;
               me->toggle_bit=0;
               me->transmission_status=cosdoc_transmission_status_pending;
               cosdoc_transmit( me, res);
               cosdoc_start_timer( me );
               me->state = cosdoc_state_sdo_upload_normal_transfer;
            }
         }
      }
      else
      {
         /* This case isn't handled yet - therefore abort */
         cosdoc_abort_request( me, COSDO_ABORT_GENERAL_PARAMETER_INCOMPATIBILITY_REASON );
      }
   }
   return RC_SUCCESS;
}

static RC cosdoc_state_sdo_upload_normal_transfer( struct cosdoc *me, U8 data[8])
{
   if( !process_scs( me, upload_segment_response, data ))
   {
      return RC_ERROR;
   }

   if( !process_toggle_bit( me, data ) )
   {
      return RC_ERROR;
   }

   {
      U8 n = 7 - ((data[0]>>1)&0x07);

      if(RC_SUCCESS == fd_write( me->file, &data[1],n) )
      {
         U8 c = !!(data[0]&0x01);
         if( 1 == c) /* No more segments to be uploaded */
         {
            if( RC_SUCCESS == fd_close( me->file, fd_close_status_success ) )
            {
               /* transmission completed */
               me->state = cosdoc_state_idle;
               me->transmission_status=cosdoc_transmission_status_completed;
               cosdoc_stop_timer( me );
            }
            else
            {
               fd_close( me->file, fd_close_status_abort );
               cosdoc_abort_request( me, COSDO_ABORT_DATA_TYPE_DOES_NOT_MATCH_LENGTH_OF_SERVICE_PARAMETER_TOO_HIGH );
            }
         }
         else
         {
            U8 res[8];
            memset(res,0,8);
            res[0]= (upload_segment_request<<5) | (me->toggle_bit<<4);
            me->transmission_status=cosdoc_transmission_status_pending;
            cosdoc_transmit( me, res );
            cosdoc_start_timer( me );
            me->state = cosdoc_state_sdo_upload_normal_transfer;
         }
      }
   }
   return RC_SUCCESS;
}


/** */
static RC cosdoc_state_initiate_sdo_download( struct cosdoc *me, U8 data[8])
{
   if( !process_scs( me, initiate_download_response, data ))
   {
      return RC_ERROR;
   }

   if( !process_mux( me, data ) )
   {
      return RC_ERROR;
   }

   return send_download_segment_request(me);
}

static RC cosdoc_state_download_sdo_segment_protocol( struct cosdoc *me, U8 data[8])
{
   if( !process_scs( me, download_segment_response, data ) )
   {
      return RC_ERROR;
   }
   if( !process_toggle_bit( me, data ) )
   {
      return RC_ERROR;
   }

   return send_download_segment_request( me );

}

static RC send_download_segment_request( struct cosdoc *me)
{
   RC rv = RC_ERROR;

   if( 0 == me->size)
   {
      cosdoc_stop_timer( me );
      me->state=cosdoc_state_idle;
      me->transmission_status = cosdoc_transmission_status_completed;
      rv=RC_SUCCESS;
   }
   else
   {
      BO last_segment = FALSE;
      U8 msg[8];
      U8 size_in_this_message=0;

      memset(&msg[0],0x00, sizeof(msg) );

      if( me->size > 7)
      {
         size_in_this_message = 7;
         last_segment = FALSE;
      }
      else
      {
         size_in_this_message = (U8) me->size;
         last_segment = TRUE;
      }

      msg[0]=(download_segment_request<<5) | (me->toggle_bit<<4) | ((7-size_in_this_message)<<1) | (last_segment);

      if( RC_SUCCESS == fd_read(me->file, &msg[1], size_in_this_message ) )
      {
         me->size-=size_in_this_message;
         cosdoc_transmit( me, msg );
         if( last_segment )
         {
            if( RC_SUCCESS == fd_close(me->file, fd_close_status_success ) )
            {
            }
            else
            {
               fd_close( me->file, fd_close_status_abort );
               cosdoc_abort_request( me, COSDO_ABORT_GENERAL_PARAMETER_INCOMPATIBILITY_REASON );
               rv=RC_ERROR;
            }
         }
         else
         {
            me->transmission_status = cosdoc_transmission_status_pending;
            me->state = cosdoc_state_download_sdo_segment_protocol;
         }
      }
      else
      {
         cosdoc_abort_request( me, COSDO_ABORT_GENERAL_PARAMETER_INCOMPATIBILITY_REASON );
         rv = RC_ERROR;
      }
   }
   return rv;
}

static BO process_mux(  struct cosdoc *me, U8 data[8] )
{
   BO rv = FALSE;
   U32 mux;

   mux = cosdoc_get_mux(data);

   if( mux == me->current_mux )
   {
      rv = TRUE;
   }
   else
   {
      rv = FALSE;
      cosdoc_abort_request( me, COSDO_ABORT_GENERAL_PARAMETER_INCOMPATIBILITY_REASON );
   }
   return rv;
}

static BO process_scs(  struct cosdoc *me, enum scs expected_scs, U8 data[8] )
{
   BO rv = FALSE;
   enum scs received_scs = data[0] >> 5;

   if( expected_scs == received_scs )
   {
      rv = TRUE;
   }
   else
   {
      rv = FALSE;
      cosdoc_abort_request( me, COSDO_ABORT_CLIENT_OR_SERVER_COMMAND_SPECIFIER_NOT_VALID_OR_UNKNOWN );
   }
   return rv;
}

static BO process_toggle_bit(  struct cosdoc *me, U8 data[8] )
{
   BO rv = FALSE;
   BO t = !!(data[0]&0x10);
   if( me->toggle_bit == t )
   {
      me->toggle_bit = !t;
      rv = TRUE;
   }
   else
   {
      cosdoc_abort_request( me , COSDO_ABORT_TOGGLE_BIT_NOT_ALTERNATED );
      rv = FALSE;
   }

   return rv;
}

static void cosdoc_abort_request(struct cosdoc *me, U32 reason )
{
   U8 data[8];
   U16 index;
   U8 subindex;

   cosdoc_stop_timer( me );

   index = (U16) (me->current_mux >> 8);
   subindex = me->current_mux & 0xff;

   endian_copy_to_little(&data[4],&reason,4);

   data[0] = cosdoc_pack_cmd(abort_transfer_request,0,0,0);
   endian_copy_to_little(&data[1],&index,2);
   data[3] = subindex;
   cosdoc_transmit( me, data );
   me->transmission_status=cosdoc_transmission_status_aborted;
   me->state=cosdoc_state_idle;

}

static RC cosdoc_state_abort_transfer_request(struct cosdoc *me, U8 data[8])
{
   U32 code;
   U32 mux;

   if( NULL == me->abort)
   {
      return RC_SUCCESS;
   }

   me->state = cosdoc_state_idle;
   mux = cosdoc_get_mux(data);
   endian_copy_from_little(&code,&data[4],4);

   return me->abort(me->abort_data,mux,code);
}

STATIC_INLINE U32 cosdoc_get_mux( U8 data[8] )
{
   U32 rv = 0;
   endian_copy_to_little( &rv, &data[1], sizeof(U16) );
   rv <<= 8;
   rv |= data[3];
   return rv;
}

static void reentrant_get(void* d, void *s, U32 size )
{
   do
   {
      memcpy(d,s,(size_t) size );
   }while( memcmp(d,s,(size_t) size ));
}

static void cosdoc_start_timer(struct cosdoc *me )
{
   me->count_down = me->timeout_max;
   me->timer_active = TRUE;
}

static void cosdoc_stop_timer(struct cosdoc *me )
{
   me->timer_active = FALSE;
}

static void cosdoc_reload_timer(struct cosdoc *me )
{
   me->count_down = me->timeout_max;
}

static U8 cosdoc_pack_cmd( U8 cmd,U8 n,BO e,U8 s)
{
   U8 head;
   head = (cmd&0x0f)<<5;
   head |= (n & 0x3 )<<2;
   head |= (!!e)<<1;
   head |= (!!s);
   return head;
}

static RC cosdoc_transmit(struct cosdoc *me, U8 data[8] )
{
   return me->transmit( me->transmit_data, data );
}

/***********************************************************************************************************************
*                             Copyright 2010 - 2013 Linde Material Handling. All rights reserved.                      *
***********************************************************************************************************************/
